package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.RobotState;
import edu.wpi.first.wpilibj.templates.subsystems.*;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.camera.AxisCameraException;

/**
 * The base for all commands. All atomic commands should subclass CommandBase.
 * CommandBase stores creates and stores each control system. To access a
 * subsystem elsewhere in your code in your code use
 * CommandBase.exampleSubsystem
 *
 * @author Author
 */
public abstract class CommandBase extends Command {

    public static OI oi;
    // Create a single static instance of all of your subsystems
    public static ArmSubsystem armSubsystem = new ArmSubsystem(RobotMap.kArmShoulderDriveMotor);
    public static ShooterSubsystem shooterSubsystem = new ShooterSubsystem(RobotMap.kShooterLoader, RobotMap.kShooterDriveMotor);
    public static ElectricSwerveSubsystem electricSwerveSubsystem = new ElectricSwerveSubsystem(RobotMap.kSwerveDriveMotor);
    public static SwerveSubsystem swerveSubsystem = electricSwerveSubsystem;
    public static DriveSubsystem leftDriveSubsystem = new DriveSubsystem("Left", RobotMap.kLeftMidDriveMotor, RobotMap.kLeftForeDriveMotor, RobotMap.kLeftRearDriveMotor);
    public static DriveSubsystem rightDriveSubsystem = new DriveSubsystem("Right", RobotMap.kRightMidDriveMotor, RobotMap.kRightForeDriveMotor, RobotMap.kRightRearDriveMotor);
    public static NewPIDLifterSubsystem rightRearLifterSubsystem = new NewPIDLifterSubsystem("RR", NewPIDLifterSubsystem.LifterID.kRightRearLifter, RobotMap.kRightRearLifterPort, RobotMap.kRightRearPot);
    public static NewPIDLifterSubsystem rightForeLifterSubsystem = new NewPIDLifterSubsystem("RF", NewPIDLifterSubsystem.LifterID.kRightForeLifter, RobotMap.kRightForeLifterPort, RobotMap.kRightForePot);
    public static NewPIDLifterSubsystem leftRearLifterSubsystem = new NewPIDLifterSubsystem("LR", NewPIDLifterSubsystem.LifterID.kLeftRearLifter, RobotMap.kLeftRearLifterPort, RobotMap.kLeftRearPot);
    public static NewPIDLifterSubsystem leftForeLifterSubsystem = new NewPIDLifterSubsystem("LF", NewPIDLifterSubsystem.LifterID.kLeftForeLifter, RobotMap.kLeftForeLifterPort, RobotMap.kLeftForePot);
   // public static VisualTrackingSubsystem AxisCamera = new VisualTrackingSubsystem("Camera", RobotMap.ip);

    public static void init() throws AxisCameraException {
        robotSpecificSetup();

        // Register Recoverable subsystems for recovery in case of power brownout
        RobotState robotState = RobotState.getInstance();
        robotState.registerForRecovery(swerveSubsystem);
        robotState.registerForRecovery(leftDriveSubsystem);
        robotState.registerForRecovery(rightDriveSubsystem);
//        robotState.registerForRecovery(shooterSubsystem);  // TBFIXED - once shooter is ready...

        // This MUST be here. If the OI creates Commands (which it very likely
        // will), constructing it during the construction of CommandBase (from
        // which commands extend), subsystems are not guaranteed to be
        // yet. Thus, their requires() statements may grab null pointers. Bad
        // news. Don't move it.

        oi = new OI();

        // Show what command your subsystem is running on the SmartDashboard
//        SmartDashboard.putData(exampleSubsystem);
        SmartDashboard.putData(leftDriveSubsystem);
        SmartDashboard.putData(rightDriveSubsystem);
        SmartDashboard.putData(swerveSubsystem);
        SmartDashboard.putData(leftForeLifterSubsystem);
        SmartDashboard.putData(leftRearLifterSubsystem);
        SmartDashboard.putData(rightForeLifterSubsystem);
        SmartDashboard.putData(rightRearLifterSubsystem);
        SmartDashboard.putData(armSubsystem);
//        SmartDashboard.putData(AxisCamera);
        
        System.out.println("------------------------------------");
        if (RobotState.RobotID.kKitty == robotState.getRobot()) {
            System.out.println("---   KITTY ROBOT IS READY       ---");
        } else if (RobotState.RobotID.kRook == robotState.getRobot()) {
            System.out.println("---   ROOK ROBOT IS READY        ---");
        } else if (RobotState.RobotID.kKitty == robotState.getRobot()) {
            System.out.println(" WARNING:  UNKNOWN ROBOT IS READY  ");
        } 
            System.out.println("------------------------------------");
    }

    public CommandBase(String name) {
        super(name);
    }

    public CommandBase() {
        super();
    }

    private static void robotSpecificSetup() throws AxisCameraException {
        System.out.println(" IDENTIFYING ROBOT");
        RobotState robotState = RobotState.getInstance();

        if (robotState.getRobot() == RobotState.RobotID.kKitty) {
            System.out.println(" ----- THIS IS KITTY -----");
            swerveSubsystem = new PneumaticSwerveSubsystem();
        } else {
            System.out.println(" ----- THIS IS ROOK -----");
            //swerveSubsystem = new ElectricSwerveSubsystem(RobotMap.kSwerveDriveMotor);
            
//        // Camera specific setup information.... might not be in right spot.
//        AxisCamera.setResolution();
//        AxisCamera.setFramesPerSecond(RobotMap.kFramesPerSecond);
//        AxisCamera.setBrightness(RobotMap.kBrightness);
//        AxisCamera.setWhiteBalance();
//        try {
//            AxisCamera.getImage();
//        } catch (NIVisionException ex) {
//            System.out.println("Camera timeout while attempting to collect "
//                    + "vision information.");
//        }
//        AxisCamera.updateDashboard();   

        }

    }
}
